﻿#include "TemplateMatch.h"
#include <QDebug>
#include <opencv2/highgui.hpp>

double match0(const cv::Mat &image, const cv::Mat &tepl, cv::Point &point, int method)
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method );
    double minVal, maxVal; cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case CV_TM_SQDIFF:
        point = minLoc;
        return minVal;
    case CV_TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;
    case CV_TM_CCORR:
    case CV_TM_CCOEFF:
        point = maxLoc;
        return maxVal;
    case CV_TM_CCORR_NORMED:
    case CV_TM_CCOEFF_NORMED:
    default: point = maxLoc;
        return maxVal;
    }
}

double match(const cv::Mat &image, const cv::Mat &tepl, cv::Point &point, int method)
{
    int result_cols = image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method );
    double minVal, maxVal; cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat() );
    switch(method)
    {
    case CV_TM_SQDIFF:
        point = minLoc;
        return minVal / (tepl.rows * tepl.cols);
    case CV_TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;
    case CV_TM_CCORR:
    case CV_TM_CCOEFF:
        point = maxLoc;
        return maxVal / (tepl.rows * tepl.cols);
    case CV_TM_CCORR_NORMED:
    case CV_TM_CCOEFF_NORMED:
    default: point = maxLoc;
        return maxVal;
    }
}

void rotate(const cv::Mat &src, cv::Mat &dst, double degree)
{
    cv::Point2f center( static_cast<float>(src.cols / 2) , static_cast<float>(src.rows / 2));
    cv::Mat affine_matrix = cv::getRotationMatrix2D( center, degree, 1.0 );
    cv::warpAffine(src, dst, affine_matrix, src.size());
}

void rotate(const cv::Mat &src, cv::Mat &dst, double degree, cv::Point2f center)
{
    cv::Mat affine_matrix = cv::getRotationMatrix2D( center, degree, 1.0 );
    cv::warpAffine(src, dst, affine_matrix, src.size());
}

void TemplateMatch::setTemplate(cv::Mat tem, double start_theta, double delta_theta, unsigned int N)
{
    m_method = 0;
    m_templates.clear();
    m_start_theta = start_theta;
    m_delta_theta = delta_theta;
    m_templates.resize(N);
    for(unsigned int i = 0; i < N; i++)
    {
        m_templates[i] = tem.clone();
        double theta = m_start_theta + i * m_delta_theta;
        rotate(tem, m_templates[i], theta);
    }
}

void TemplateMatch::setTemplate(cv::Mat tem, double delta_theta, unsigned int N)
{
    m_method = 1;
    m_templates.clear();
    int n = static_cast<int>(2 * N + 1);
    m_templates.resize(n);

    m_start_theta = - delta_theta * N;
    m_delta_theta = delta_theta;

    for(unsigned int i = 0; i < n; i++)
    {
        m_templates[i] = tem.clone();
        double theta = m_start_theta + i * m_delta_theta;
        rotate(tem, m_templates[i], theta);
    }
}

double TemplateMatch::match1(const cv::Mat &image, cv::Point2f &center, double &theta, int method)
{
    unsigned int N = m_templates.size();

    cv::Point point, bestPoint;
    double best = ::match(image, m_templates[0], bestPoint, method);
    unsigned int bestN = 0;
    for(unsigned int i = 1; i < N ; i++)
    {
       double value = ::match(image, m_templates[i], point, method);
       if(better(best, value, method))
       {
           bestPoint = point;
           best = value;
           bestN = i;
       }
    }
    center.x = static_cast<float>(bestPoint.x + m_templates[0].cols / 2.0);
    center.y = static_cast<float>(bestPoint.y + m_templates[0].rows / 2.0);
    theta = m_start_theta + m_delta_theta * bestN;

    return best;
}

double TemplateMatch::match2(const cv::Mat &image, cv::Point2f &center, double &theta, int method)
{
    int n = static_cast<int>(m_templates.size());
    int N = (n - 1) / 2;
    cv::Point point, bestPoint;
    double grade0 = ::match(image, m_templates[N], bestPoint, method);
    bool rightDirection = false;
    double grade, grade_final = grade0;
    int count = N;
    for(int i = N + 1; i < n; i++) // 先正转, 如果方向对的，结果应该越来越好
   {
        grade = ::match(image, m_templates[i], point, method);
        if(better(grade_final, grade, method))
        {
            rightDirection = true; //表明旋转方式是对的，就不用反方向转了
            bestPoint = point;
            grade_final = grade;
            count = i;
        }
        else
        {
            break;
        }
    }
    if(!rightDirection)//如果正转没有找到合适的角度
    {
        for(int i = N - 1; i >= 0; i--) // 倒转
        {
            grade = ::match(image, m_templates[i], point, method);
            if(better(grade_final, grade, method))
            {
                rightDirection = true; //表明旋转方式是对的
                bestPoint = point;
                grade_final = grade;
                count = i;
            }
            else
            {
                break;
            }
        }
    }

    if(rightDirection)//已经确定了旋转角度
    {
        theta = m_start_theta + count * m_delta_theta;
    }
    else
    {
        theta = 0;
    }
    center.x = static_cast<float>(bestPoint.x + m_templates[0].cols / 2.0);
    center.y = static_cast<float>(bestPoint.y + m_templates[0].rows / 2.0);

    return grade_final;
}

double TemplateMatch::match3(const cv::Mat &image, cv::Point2f &center, double &theta, int method) //先正转，再反转
{
    int n = static_cast<int>(m_templates.size());
    int N = (n - 1) / 2;
    cv::Point point, bestPoint;
    double grade = ::match(image, m_templates[N], bestPoint, method);
    //qDebug() << "grade = " << grade;
    int startX = bestPoint.x - 50; if(startX < 0) startX = 0;
    int startY = bestPoint.y - 50; if(startY < 0) startY = 0;
    int endX = bestPoint.x + m_templates[N].cols + 50;
    int endY = bestPoint.y + m_templates[N].rows + 50;
    if(endX >= image.cols) endX = image.cols - 1;
    if(endY >= image.rows) endY = image.rows - 1;

    cv::Mat imageSmall = image(cv::Rect(cv::Point(startX, startY), cv::Point(endX, endY)));
    grade = match1(imageSmall, center, theta, method);

    center.x = center.x + startX;
    center.y = center.y + startY;
    return grade;
}

bool TemplateMatch::better(double oldGrade, double newGrade, int method)
{
    switch (method)
    {
    case CV_TM_SQDIFF:
    case CV_TM_SQDIFF_NORMED:
        if(newGrade < oldGrade)
        {
            return true;
        }
         break;
    default:
        if(newGrade > oldGrade)
        {
            return true;
        }
    }
    return false;
}

double TemplateMatch::match(cv::Mat image, cv::Point2f &center, double &theta, int method)
{
    switch (m_method)
    {
    case 0:
        return match1(image, center, theta, method);
    case 1:
        return match2(image, center, theta, method);
    case 2:
        return match3(image, center, theta, method);
    default:
        return match1(image, center, theta, method);
    }
}

bool TemplateMatch::showTemplate(double theta)
{
    int N = m_templates.size();
    int i = static_cast<int>((theta - m_start_theta) / m_delta_theta + 0.5); // 四舍五入

    if( i >= 0 && i < N )
    {
        cv::imshow("template", m_templates[i]);
        return true;
    }
    return false;
}

void TemplateMatch::showTemplate()
{
    for(unsigned int i = 0; i < m_templates.size(); i++)
    {
        std::string s(1, '0' + i);
        cv::imshow(s, m_templates[i]);
    }
}
TemplateMatch::TemplateMatch()
{
    m_method = 0;
}
